
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_gps_nmea.c
  * @author     baiyang
  * @date       2023-2-14
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_gps_nmea.h"

#include <ctype.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <float.h>

#include <logger/ap_log.h>
#include <gcs_mavlink/gcs.h>
#include <common/time/gp_time.h>
#include <common/console/console.h>
#include <common/gp_math/gp_mathlib.h>
#include <board_config/board_config.h>
/*-----------------------------------macro------------------------------------*/
#if MB_GPS_NMEA_ENABLED

#ifndef MB_GPS_NMEA_CONFIG_PERIOD_MS
// how often we send board specific config commands
#define MB_GPS_NMEA_CONFIG_PERIOD_MS 15000U
#endif

// a quiet nan for invalid values
#define QNAN nanf("GPS")

#define DIGIT_TO_VAL(_x)        (_x - '0')
#define hexdigit(x) ((x)>9?'A'+((x)-10):'0'+(x))
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool _decode(sensor_gps_nmea_t gps_nmea, char c);
static int32_t _parse_decimal_100(const char *p);
static uint32_t _parse_degrees(sensor_gps_nmea_t gps_nmea);
static bool _have_new_message(sensor_gps_nmea_t gps_nmea);
static bool _term_complete(sensor_gps_nmea_t gps_nmea);
#if MB_GPS_NMEA_UNICORE_ENABLED
static void parse_agrica_field(sensor_gps_nmea_t gps_nmea, uint16_t term_number, const char *term);
static void parse_versiona_field(sensor_gps_nmea_t gps_nmea, uint16_t term_number, const char *term);
#if GPS_MOVING_BASELINE
static void parse_uniheadinga_field(sensor_gps_nmea_t gps_nmea, uint16_t term_number, const char *term);
#endif
#endif
static void send_config(sensor_gps_nmea_t gps_nmea);

static bool nmea_read(sensor_gps_backend_t gps_backend);
static const char *nmea_name();
static bool nmea_is_healthy(sensor_gps_backend_t gps_backend);
static bool nmea_get_lag(sensor_gps_backend_t gps_backend, float *lag_sec);
/*----------------------------------variable----------------------------------*/
static struct sensor_gps_backend_ops gps_nmea_ops = {.gps_backend_destructor    = NULL,
                                                     .read                   = nmea_read,
                                                     .highest_supported_status = NULL,
                                                     .is_configured          = NULL,
                                                     .inject_data            = NULL,
                                                     .supports_mavlink_gps_rtk_message = NULL,
                                                     .send_mavlink_gps_rtk   = NULL,
                                                     .broadcast_configuration_failure_reason = NULL,
                                                     .handle_msg             = NULL,
                                                     .get_lag                = nmea_get_lag,
                                                     .is_healthy             = nmea_is_healthy,
                                                     .logging_healthy        = NULL,
                                                     .name                   = nmea_name,
                                                     .Write_AP_Logger_Log_Startup_messages = NULL,
                                                     .prepare_for_arming     = NULL,
                                                     .get_RTCMV3             = NULL,
                                                     .clear_RTCMV3           = NULL,
                                                     .get_error_codes        = NULL};

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_gps_nmea_ctor(sensor_gps_nmea_t gps_nmea, struct GPS_State *_state, rt_device_t _port)
{
    // 清空sensor_gps_nmea结构体变量，因为sensor_gps_ublox结构体有可能是申请的动态内存
    // 防止sensor_gps_nmea中的变量初始为非零值。
    rt_memset(gps_nmea, 0, sizeof(struct sensor_gps_nmea));

    sensor_gps_backend_ctor(&gps_nmea->backend, &gps_nmea_ops, _state, _port);
}

sensor_gps_backend_t sensor_gps_nmea_probe(struct GPS_State *_state, rt_device_t _port)
{
    sensor_gps_nmea_t gps_nmea = (sensor_gps_nmea_t)rt_malloc(sizeof(struct sensor_gps_nmea));

    if (gps_nmea != NULL) {
        sensor_gps_nmea_ctor(gps_nmea, _state, _port);
        return (sensor_gps_backend_t)gps_nmea;
    }

    return NULL;
}

/*
  detect a NMEA GPS. Adds one byte, and returns true if the stream
  matches a NMEA string
 */
bool sensor_gps_nmea_detect(struct NMEA_detect_state *state, uint8_t data)
{
    switch (state->step) {
    case 0:
        state->ck = 0;
        if ('$' == data) {
            state->step++;
        }
        break;
    case 1:
        if ('*' == data) {
            state->step++;
        } else {
            state->ck ^= data;
        }
        break;
    case 2:
        if (hexdigit(state->ck>>4) == data) {
            state->step++;
        } else {
            state->step = 0;
        }
        break;
    case 3:
        if (hexdigit(state->ck&0xF) == data) {
            state->step = 0;
            return true;
        }
        state->step = 0;
        break;
    }
    return false;
}

/*
  decode one character, return true if we have successfully completed a sentence, false otherwise
 */
static bool _decode(sensor_gps_nmea_t gps_nmea, char c)
{
    gps_nmea->_sentence_length++;

    switch (c) {
    case ';':
        // header separator for unicore
        if (!gps_nmea->_is_unicore) {
            return false;
        }
    case ',': // term terminators
        gps_nmea->_parity ^= c;
        if (gps_nmea->_is_unicore) {
            gps_nmea->_crc32 = math_crc_crc32(gps_nmea->_crc32, (const uint8_t *)&c, 1);
        }
    case '\r':
    case '\n':
    case '*': {
        if (gps_nmea->_sentence_done) {
            return false;
        }
        bool valid_sentence = false;
        if (gps_nmea->_term_offset < sizeof(gps_nmea->_term)) {
            gps_nmea->_term[gps_nmea->_term_offset] = 0;
            valid_sentence = _term_complete(gps_nmea);
        }
        ++gps_nmea->_term_number;
        gps_nmea->_term_offset = 0;
        gps_nmea->_is_checksum_term = c == '*';
        return valid_sentence;
    }

    case '$': // sentence begin
    case '#': // unicore message begin
        gps_nmea->_is_unicore = (c == '#');
        gps_nmea->_term_number = gps_nmea->_term_offset = 0;
        gps_nmea->_parity = 0;
        gps_nmea->_crc32 = 0;
        gps_nmea->_sentence_type = _GPS_SENTENCE_OTHER;
        gps_nmea->_is_checksum_term = false;
        gps_nmea->_sentence_length = 1;
        gps_nmea->_sentence_done = false;
        gps_nmea->_new_gps_yaw = QNAN;
        return false;
    }

    // ordinary characters
    if (gps_nmea->_term_offset < sizeof(gps_nmea->_term) - 1)
        gps_nmea->_term[gps_nmea->_term_offset++] = c;
    if (!gps_nmea->_is_checksum_term) {
        gps_nmea->_parity ^= c;
        if (gps_nmea->_is_unicore) {
            gps_nmea->_crc32 = math_crc_crc32(gps_nmea->_crc32, (const uint8_t *)&c, 1);
        }
    }

    return false;
}

static int32_t _parse_decimal_100(const char *p)
{
    char *endptr = NULL;
    long ret = 100 * strtol(p, &endptr, 10);
    int sign = ret < 0 ? -1 : 1;

    if (ret >= (long)INT32_MAX) {
        return INT32_MAX;
    }
    if (ret <= (long)INT32_MIN) {
        return INT32_MIN;
    }
    if (endptr == NULL || *endptr != '.') {
        return ret;
    }

    if (isdigit(endptr[1])) {
        ret += sign * 10 * DIGIT_TO_VAL(endptr[1]);
        if (isdigit(endptr[2])) {
            ret += sign * DIGIT_TO_VAL(endptr[2]);
            if (isdigit(endptr[3])) {
                ret += sign * (DIGIT_TO_VAL(endptr[3]) >= 5);
            }
        }
    }
    return ret;
}

/*
  parse a NMEA latitude/longitude degree value. The result is in degrees*1e7
 */
static uint32_t _parse_degrees(sensor_gps_nmea_t gps_nmea)
{
    char *p, *q;
    uint8_t deg = 0, min = 0;
    float frac_min = 0;
    int32_t ret = 0;

    // scan for decimal point or end of field
    for (p = gps_nmea->_term; *p && isdigit(*p); p++)
        ;
    q = gps_nmea->_term;

    // convert degrees
    while ((p - q) > 2 && *q) {
        if (deg)
            deg *= 10;
        deg += DIGIT_TO_VAL(*q++);
    }

    // convert minutes
    while (p > q && *q) {
        if (min)
            min *= 10;
        min += DIGIT_TO_VAL(*q++);
    }

    // convert fractional minutes
    if (*p == '.') {
        q = p + 1;
        float frac_scale = 0.1f;
        while (*q && isdigit(*q)) {
            frac_min += DIGIT_TO_VAL(*q) * frac_scale;
            q++;
            frac_scale *= 0.1f;
        }
    }
    ret = (deg * (int32_t)10000000UL);
    ret += (min * (int32_t)10000000UL / 60);
    ret += (int32_t) (frac_min * (1.0e7f / 60.0f));
    return ret;
}

/*
  see if we have a new set of NMEA messages
 */
static bool _have_new_message(sensor_gps_nmea_t gps_nmea)
{
    sensor_gps_backend_t gps_backend = &gps_nmea->backend;

    if (gps_nmea->_last_RMC_ms == 0 ||
        gps_nmea->_last_GGA_ms == 0) {
        return false;
    }
    uint32_t now = time_millis();
    if (now - gps_nmea->_last_RMC_ms > 150 ||
        now - gps_nmea->_last_GGA_ms > 150) {
        return false;
    }
    if (gps_nmea->_last_VTG_ms != 0 && 
        now - gps_nmea->_last_VTG_ms > 150) {
        return false;
    }

    /*
      if we have seen a message with 3D velocity data messages then
      wait for them again. This is important as the
      have_vertical_velocity field will be overwritten by
      fill_3d_velocity()
     */
    if (gps_nmea->_last_vvelocity_ms != 0 &&
        now - gps_nmea->_last_vvelocity_ms > 150 &&
        now - gps_nmea->_last_vvelocity_ms < 1000) {
        // waiting on a message with velocity
        return false;
    }
    if (gps_nmea->_last_vaccuracy_ms != 0 &&
        now - gps_nmea->_last_vaccuracy_ms > 150 &&
        now - gps_nmea->_last_vaccuracy_ms < 1000) {
        // waiting on a message with velocity accuracy
        return false;
    }

    // prevent these messages being used again
    if (gps_nmea->_last_VTG_ms != 0) {
        gps_nmea->_last_VTG_ms = 1;
    }

    if (now - gps_nmea->_last_yaw_ms > 300) {
        // we have lost GPS yaw
        gps_backend->state->have_gps_yaw = false;
    }

    if (now - gps_nmea->_last_KSXT_pos_ms > 500) {
        // we have lost KSXT
        gps_nmea->_last_KSXT_pos_ms = 0;
    }

#if MB_GPS_NMEA_UNICORE_ENABLED
    if (now - gps_nmea->_last_AGRICA_ms > 500) {
        if (gps_nmea->_last_AGRICA_ms != 0) {
            // we have lost AGRICA
            gps_backend->state->have_gps_yaw = false;
            gps_backend->state->have_horizontal_velocity = false;
            gps_backend->state->have_vertical_velocity = false;
            gps_backend->state->have_speed_accuracy = false;
            gps_backend->state->have_horizontal_accuracy = false;
            gps_backend->state->have_vertical_accuracy = false;
            gps_backend->state->have_undulation = false;
            gps_nmea->_last_AGRICA_ms = 0;
        }
    }
#endif // MB_GPS_NMEA_UNICORE_ENABLED

    gps_nmea->_last_fix_ms = now;

    gps_nmea->_last_GGA_ms = 1;
    gps_nmea->_last_RMC_ms = 1;
    return true;
}

// Processes a just-completed term
// Returns true if new sentence has just passed checksum test and is validated
static bool _term_complete(sensor_gps_nmea_t gps_nmea)
{
    sensor_gps_backend_t gps_backend = &gps_nmea->backend;

    // handle the last term in a message
    if (gps_nmea->_is_checksum_term) {
        gps_nmea->_sentence_done = true;
        const uint32_t crc = strtoul(gps_nmea->_term, NULL, 16);
        const bool crc_ok = gps_nmea->_is_unicore? (gps_nmea->_crc32 == crc) : (gps_nmea->_parity == crc);
        if (crc_ok) {
            uint64_t now_us = time_micros64();
            uint32_t now = now_us/1000;
            switch (gps_nmea->_sentence_type) {
            case _GPS_SENTENCE_RMC:
                gps_nmea->_last_RMC_ms = now;
                //time                        = _new_time;
                //date                        = _new_date;
                if (gps_nmea->_last_KSXT_pos_ms == 0 && gps_nmea->_last_AGRICA_ms == 0) {
                    gps_backend->state->location.lat     = gps_nmea->_new_latitude;
                    gps_backend->state->location.lng     = gps_nmea->_new_longitude;
                }
                if (gps_nmea->_last_3D_velocity_ms == 0 ||
                    now - gps_nmea->_last_3D_velocity_ms > 1000) {
                    gps_backend->state->ground_speed     = gps_nmea->_new_speed*0.01f;
                    gps_backend->state->ground_course    = math_wrap_360(gps_nmea->_new_course*0.01f);
                }
                if (gps_backend->state->status >= GPS_OK_FIX_3D) {
                    sensor_gps_backend_make_gps_time(gps_backend, gps_nmea->_new_date, gps_nmea->_new_time * 10);
                    if (gps_nmea->_last_AGRICA_ms != 0) {
                        gps_backend->state->time_week_ms = gps_backend->_last_itow_ms;
                    }
                }
                sensor_gps_backend_set_uart_timestamp(gps_backend, gps_nmea->_sentence_length);
                gps_backend->state->last_gps_time_us = now_us;
                gps_backend->state->last_gps_time_ms = now;
                if (gps_nmea->_last_vvelocity_ms == 0 ||
                    now - gps_nmea->_last_vvelocity_ms > 1000) {
                    sensor_gps_backend_fill_3d_velocity(gps_backend);
                }
                break;
            case _GPS_SENTENCE_GGA:
                gps_nmea->_last_GGA_ms = now;
                if (gps_nmea->_last_KSXT_pos_ms == 0 && gps_nmea->_last_AGRICA_ms == 0) {
                    gps_backend->state->location.alt  = gps_nmea->_new_altitude;
                    gps_backend->state->location.lat  = gps_nmea->_new_latitude;
                    gps_backend->state->location.lng  = gps_nmea->_new_longitude;
                }
                gps_backend->state->num_sats      = gps_nmea->_new_satellite_count;
                gps_backend->state->hdop          = gps_nmea->_new_hdop;
                switch(gps_nmea->_new_quality_indicator) {
                case 0: // Fix not available or invalid
                    gps_backend->state->status = NO_FIX;
                    break;
                case 1: // GPS SPS Mode, fix valid
                    gps_backend->state->status = GPS_OK_FIX_3D;
                    break;
                case 2: // Differential GPS, SPS Mode, fix valid
                    gps_backend->state->status = GPS_OK_FIX_3D_DGPS;
                    break;
                case 3: // GPS PPS Mode, fix valid
                    gps_backend->state->status = GPS_OK_FIX_3D;
                    break;
                case 4: // Real Time Kinematic. System used in RTK mode with fixed integers
                    gps_backend->state->status = GPS_OK_FIX_3D_RTK_FIXED;
                    break;
                case 5: // Float RTK. Satellite system used in RTK mode, floating integers
                    gps_backend->state->status = GPS_OK_FIX_3D_RTK_FLOAT;
                    break;
                case 6: // Estimated (dead reckoning) Mode
                    gps_backend->state->status = NO_FIX;
                    break;
                default://to maintain compatibility with MAV_GPS_INPUT and others
                    gps_backend->state->status = GPS_OK_FIX_3D;
                    break;
                }
                break;
            case _GPS_SENTENCE_VTG:
                gps_nmea->_last_VTG_ms = now;
                if (gps_nmea->_last_3D_velocity_ms == 0 ||
                    now - gps_nmea->_last_3D_velocity_ms > 1000) {
                    gps_backend->state->ground_speed  = gps_nmea->_new_speed*0.01f;
                    gps_backend->state->ground_course = math_wrap_360(gps_nmea->_new_course*0.01f);
                    if (gps_nmea->_last_vvelocity_ms == 0 ||
                        now - gps_nmea->_last_vvelocity_ms > 1000) {
                        sensor_gps_backend_fill_3d_velocity(gps_backend);
                    }
                }
                // VTG has no fix indicator, can't change fix status
                break;
            case _GPS_SENTENCE_HDT:
            case _GPS_SENTENCE_THS:
                if (gps_nmea->_last_AGRICA_ms != 0 || gps_nmea->_expect_agrica) {
                    // use AGRICA
                    break;
                }
                if (isnan(gps_nmea->_new_gps_yaw)) {
                    // empty sentence
                    break;
                }
                gps_nmea->_last_yaw_ms = now;
                gps_backend->state->gps_yaw = math_wrap_360(gps_nmea->_new_gps_yaw*0.01f);
                gps_backend->state->have_gps_yaw = true;
                gps_backend->state->gps_yaw_time_ms = now;
                // remember that we are setup to provide yaw. With
                // a NMEA GPS we can only tell if the GPS is
                // configured to provide yaw when it first sends a
                // HDT sentence.
                gps_backend->state->gps_yaw_configured = true;
                break;
            case _GPS_SENTENCE_PHD:
                if (gps_nmea->_last_AGRICA_ms != 0) {
                    // prefer AGRICA
                    break;
                }
                if (gps_nmea->_phd.msg_id == 12) {
                    gps_backend->state->velocity.x = gps_nmea->_phd.fields[0] * 0.01f;
                    gps_backend->state->velocity.y = gps_nmea->_phd.fields[1] * 0.01f;
                    gps_backend->state->velocity.z = gps_nmea->_phd.fields[2] * 0.01f;
                    gps_backend->state->have_horizontal_velocity = true;
                    gps_backend->state->have_vertical_velocity = true;
                    gps_nmea->_last_vvelocity_ms = now;
                    // we prefer a true 3D velocity when available
                    sensor_gps_backend_velocity_to_speed_course(gps_backend, gps_backend->state);
                    gps_nmea->_last_3D_velocity_ms = now;
                } else if (gps_nmea->_phd.msg_id == 26) {
                    gps_backend->state->horizontal_accuracy = MAX(gps_nmea->_phd.fields[0],gps_nmea->_phd.fields[1]) * 0.001f;
                    gps_backend->state->have_horizontal_accuracy = true;
                    gps_backend->state->vertical_accuracy = gps_nmea->_phd.fields[2] * 0.001f;
                    gps_backend->state->have_vertical_accuracy = true;
                    gps_backend->state->speed_accuracy = MAX(gps_nmea->_phd.fields[3],gps_nmea->_phd.fields[4]) * 0.001f;
                    gps_backend->state->have_speed_accuracy = true;
                    gps_nmea->_last_vaccuracy_ms = now;
                }
                break;
            case _GPS_SENTENCE_KSXT:
                if (gps_nmea->_last_AGRICA_ms != 0 || gps_nmea->_expect_agrica) {
                    // prefer AGRICA
                    break;
                }
                gps_backend->state->location.lat     = gps_nmea->_ksxt.fields[2]*1.0e7f;
                gps_backend->state->location.lng     = gps_nmea->_ksxt.fields[1]*1.0e7f;
                gps_backend->state->location.alt     = gps_nmea->_ksxt.fields[3]*1.0e2f;
                gps_nmea->_last_KSXT_pos_ms = now;
                if (gps_nmea->_ksxt.fields[9] >= 1) {
                    // we have 3D fix
                    const float kmh_to_mps = 1.0f / 3.6f;
                    gps_backend->state->velocity.y = gps_nmea->_ksxt.fields[16] * kmh_to_mps;
                    gps_backend->state->velocity.x = gps_nmea->_ksxt.fields[17] * kmh_to_mps;
                    gps_backend->state->velocity.z = gps_nmea->_ksxt.fields[18] * -kmh_to_mps;
                    gps_backend->state->have_horizontal_velocity = true;
                    gps_backend->state->have_vertical_velocity = true;
                    gps_nmea->_last_vvelocity_ms = now;
                    // we prefer a true 3D velocity when available
                    sensor_gps_backend_velocity_to_speed_course(gps_backend, gps_backend->state);
                    gps_nmea->_last_3D_velocity_ms = now;
                }
                if (math_flt_equal(3.0f, (float)(gps_nmea->_ksxt.fields[10]))) {
                    // have good yaw (from RTK fixed moving baseline solution)
                    gps_nmea->_last_yaw_ms = now;
                    gps_backend->state->gps_yaw = gps_nmea->_ksxt.fields[4];
                    gps_backend->state->have_gps_yaw = true;
                    gps_backend->state->gps_yaw_time_ms = now;
                    gps_backend->state->gps_yaw_configured = true;
                }
                break;
#if MB_GPS_NMEA_UNICORE_ENABLED
            case _GPS_SENTENCE_AGRICA: {
                gps_nmea->_last_AGRICA_ms = now;
                gps_nmea->_last_vvelocity_ms = now;
                gps_nmea->_last_vaccuracy_ms = now;
                gps_nmea->_last_3D_velocity_ms = now;
                gps_backend->state->location.lat = gps_nmea->_agrica.lat*1.0e7;
                gps_backend->state->location.lng = gps_nmea->_agrica.lng*1.0e7;
                gps_backend->state->location.alt = gps_nmea->_agrica.alt*1.0e2;
                gps_backend->state->undulation   = -gps_nmea->_agrica.undulation;
                gps_backend->state->velocity = gps_nmea->_agrica.vel_NED;
                sensor_gps_backend_velocity_to_speed_course(gps_backend, gps_backend->state);
                gps_backend->state->speed_accuracy = vec3_length(&gps_nmea->_agrica.vel_stddev);
                gps_backend->state->horizontal_accuracy = vec3_length_xy(&gps_nmea->_agrica.pos_stddev);
                gps_backend->state->vertical_accuracy = gps_nmea->_agrica.pos_stddev.z;
                gps_backend->state->have_horizontal_velocity = true;
                gps_backend->state->have_vertical_velocity = true;
                gps_backend->state->have_speed_accuracy = true;
                gps_backend->state->have_horizontal_accuracy = true;
                gps_backend->state->have_vertical_accuracy = true;
                gps_backend->state->have_undulation = true;
                sensor_gps_backend_check_new_itow(gps_backend, gps_nmea->_agrica.itow, gps_nmea->_sentence_length);
                break;
            }
            case _GPS_SENTENCE_VERSIONA: {
                gps_nmea->_have_unicore_versiona = true;
                GCS_SEND_TEXT(MAV_SEVERITY_INFO,
                              "NMEA %s %s %s",
                              gps_nmea->_versiona.type,
                              gps_nmea->_versiona.version,
                              gps_nmea->_versiona.build_date);
                break;
            }
            case _GPS_SENTENCE_UNIHEADINGA: {
#if GPS_MOVING_BASELINE
                if (now - gps_nmea->_last_AGRICA_ms > 500 || gps_nmea->_agrica.heading_status != 4) {
                    // we need heading_status from AGRICA
                    gps_backend->state->have_gps_yaw = false;
                    break;
                }
                const float dist = gps_nmea->_uniheadinga.baseline_length;
                const float bearing = gps_nmea->_uniheadinga.heading;
                const float alt_diff = dist*tanf(radians(-gps_nmea->_uniheadinga.pitch));
                gps_backend->state->relPosHeading = bearing;
                gps_backend->state->relPosLength = dist;
                gps_backend->state->relPosD = alt_diff;
                gps_backend->state->relposheading_ts = now;
                if (sensor_gps_backend_calculate_moving_base_yaw(gps_backend, bearing, dist, alt_diff)) {
                    gps_backend->state->have_gps_yaw_accuracy = true;
                    gps_backend->state->gps_yaw_accuracy = gps_nmea->_uniheadinga.heading_sd;
                    gps_nmea->_last_yaw_ms = now;
                }
                gps_backend->state->gps_yaw_configured = true;
#endif // GPS_MOVING_BASELINE
                break;
            }
#endif // AP_GPS_NMEA_UNICORE_ENABLED
            }
            // see if we got a good message
            return _have_new_message(gps_nmea);
        } else {
            aplog_write_message("NMEA: crc failure");
        }
        // we got a bad message, ignore it
        return false;
    }

    // the first term determines the sentence type
    if (gps_nmea->_term_number == 0) {
        /*
          special case for $PHD message
         */
        if (strcmp(gps_nmea->_term, "PHD") == 0) {
            gps_nmea->_sentence_type = _GPS_SENTENCE_PHD;
            return false;
        }
        if (strcmp(gps_nmea->_term, "KSXT") == 0) {
            gps_nmea->_sentence_type = _GPS_SENTENCE_KSXT;
            return false;
        }
#if MB_GPS_NMEA_UNICORE_ENABLED
        if (strcmp(gps_nmea->_term, "AGRICA") == 0 && gps_nmea->_expect_agrica) {
            gps_nmea->_sentence_type = _GPS_SENTENCE_AGRICA;
            return false;
        }
        if (strcmp(gps_nmea->_term, "VERSIONA") == 0) {
            gps_nmea->_sentence_type = _GPS_SENTENCE_VERSIONA;
            return false;
        }
        if (strcmp(gps_nmea->_term, "UNIHEADINGA") == 0 && gps_nmea->_expect_agrica) {
            gps_nmea->_sentence_type = _GPS_SENTENCE_UNIHEADINGA;
            return false;
        }
#endif
        /*
          The first two letters of the NMEA term are the talker
          ID. The most common is 'GP' but there are a bunch of others
          that are valid. We accept any two characters here.
         */
        if (gps_nmea->_term[0] < 'A' || gps_nmea->_term[0] > 'Z' ||
            gps_nmea->_term[1] < 'A' || gps_nmea->_term[1] > 'Z') {
            gps_nmea->_sentence_type = _GPS_SENTENCE_OTHER;
            return false;
        }
        const char *term_type = &gps_nmea->_term[2];
        if (strcmp(term_type, "RMC") == 0) {
            gps_nmea->_sentence_type = _GPS_SENTENCE_RMC;
        } else if (strcmp(term_type, "GGA") == 0) {
            gps_nmea->_sentence_type = _GPS_SENTENCE_GGA;
        } else if (strcmp(term_type, "HDT") == 0) {
            gps_nmea->_sentence_type = _GPS_SENTENCE_HDT;
        } else if (strcmp(term_type, "THS") == 0) {
            gps_nmea->_sentence_type = _GPS_SENTENCE_THS;
        } else if (strcmp(term_type, "VTG") == 0) {
            gps_nmea->_sentence_type = _GPS_SENTENCE_VTG;
        } else {
            gps_nmea->_sentence_type = _GPS_SENTENCE_OTHER;
        }
        return false;
    }

    // 32 = RMC, 64 = GGA, 96 = VTG, 128 = HDT, 160 = THS
    if (gps_nmea->_sentence_type != _GPS_SENTENCE_OTHER && gps_nmea->_term[0]) {
        switch (gps_nmea->_sentence_type + gps_nmea->_term_number) {
        // operational status
        //
        case _GPS_SENTENCE_RMC + 2: // validity (RMC)
            break;
        case _GPS_SENTENCE_GGA + 6: // Fix data (GGA)
            if (gps_nmea->_term[0] > '0') {
                gps_nmea->_new_quality_indicator = gps_nmea->_term[0] - '0';
            } else {
                gps_nmea->_new_quality_indicator = 0;
            }
            break;
        case _GPS_SENTENCE_GGA + 7: // satellite count (GGA)
            gps_nmea->_new_satellite_count = atol(gps_nmea->_term);
            break;
        case _GPS_SENTENCE_GGA + 8: // HDOP (GGA)
            gps_nmea->_new_hdop = (uint16_t)_parse_decimal_100(gps_nmea->_term);
            break;

        // time and date
        //
        case _GPS_SENTENCE_RMC + 1: // Time (RMC)
        case _GPS_SENTENCE_GGA + 1: // Time (GGA)
            gps_nmea->_new_time = _parse_decimal_100(gps_nmea->_term);
            break;
        case _GPS_SENTENCE_RMC + 9: // Date (GPRMC)
            gps_nmea->_new_date = atol(gps_nmea->_term);
            break;

        // location
        //
        case _GPS_SENTENCE_RMC + 3: // Latitude
        case _GPS_SENTENCE_GGA + 2:
            gps_nmea->_new_latitude = _parse_degrees(gps_nmea);
            break;
        case _GPS_SENTENCE_RMC + 4: // N/S
        case _GPS_SENTENCE_GGA + 3:
            if (gps_nmea->_term[0] == 'S')
                gps_nmea->_new_latitude = -gps_nmea->_new_latitude;
            break;
        case _GPS_SENTENCE_RMC + 5: // Longitude
        case _GPS_SENTENCE_GGA + 4:
            gps_nmea->_new_longitude = _parse_degrees(gps_nmea);
            break;
        case _GPS_SENTENCE_RMC + 6: // E/W
        case _GPS_SENTENCE_GGA + 5:
            if (gps_nmea->_term[0] == 'W')
                gps_nmea->_new_longitude = -gps_nmea->_new_longitude;
            break;
        case _GPS_SENTENCE_GGA + 9: // Altitude (GPGGA)
            gps_nmea->_new_altitude = _parse_decimal_100(gps_nmea->_term);
            break;

        // course and speed
        //
        case _GPS_SENTENCE_RMC + 7: // Speed (GPRMC)
        case _GPS_SENTENCE_VTG + 5: // Speed (VTG)
            gps_nmea->_new_speed = (_parse_decimal_100(gps_nmea->_term) * 514) / 1000;       // knots-> m/sec, approximiates * 0.514
            break;
        case _GPS_SENTENCE_HDT + 1: // Course (HDT)
            gps_nmea->_new_gps_yaw = _parse_decimal_100(gps_nmea->_term);
            break;
        case _GPS_SENTENCE_THS + 1: // Course (THS)
            gps_nmea->_new_gps_yaw = _parse_decimal_100(gps_nmea->_term);
            break;
        case _GPS_SENTENCE_RMC + 8: // Course (GPRMC)
        case _GPS_SENTENCE_VTG + 1: // Course (VTG)
            gps_nmea->_new_course = _parse_decimal_100(gps_nmea->_term);
            break;

        case _GPS_SENTENCE_PHD + 1: // PHD class
            gps_nmea->_phd.msg_class = atol(gps_nmea->_term);
            break;
        case _GPS_SENTENCE_PHD + 2: // PHD message
            gps_nmea->_phd.msg_id = atol(gps_nmea->_term);
            break;
        case _GPS_SENTENCE_PHD + 5: // PHD message, itow
            gps_nmea->_phd.itow = strtoul(gps_nmea->_term, NULL, 10);
            break;
        case _GPS_SENTENCE_PHD + 6 ... _GPS_SENTENCE_PHD + 11: // PHD message, fields
            gps_nmea->_phd.fields[gps_nmea->_term_number-6] = atol(gps_nmea->_term);
            break;
        case _GPS_SENTENCE_KSXT + 1 ... _GPS_SENTENCE_KSXT + 22: // KSXT message, fields
            gps_nmea->_ksxt.fields[gps_nmea->_term_number-1] = atof(gps_nmea->_term);
            break;
#if MB_GPS_NMEA_UNICORE_ENABLED
        case _GPS_SENTENCE_AGRICA + 1 ... _GPS_SENTENCE_AGRICA + 65: // AGRICA message
            parse_agrica_field(gps_nmea, gps_nmea->_term_number, gps_nmea->_term);
            break;
        case _GPS_SENTENCE_VERSIONA + 1 ... _GPS_SENTENCE_VERSIONA + 20:
            parse_versiona_field(gps_nmea, gps_nmea->_term_number, gps_nmea->_term);
            break;
#if GPS_MOVING_BASELINE
        case _GPS_SENTENCE_UNIHEADINGA + 1 ... _GPS_SENTENCE_UNIHEADINGA + 28: // UNIHEADINGA message
            parse_uniheadinga_field(gps_nmea, gps_nmea->_term_number, gps_nmea->_term);
            break;
#endif
#endif
        }
    }

    return false;
}

#if MB_GPS_NMEA_UNICORE_ENABLED
/*
  parse an AGRICA message term

  Example:
     #AGRICA,82,GPS,FINE,2237,176366400,0,0,18,15;GNSS,232,22,11,22,0,59,8,1,5,8,12,0,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,296.4656,-26.5685,0.0000,0.005,0.000,0.000,-0.005,0.044,0.032,0.038,-35.33142715815,149.13181842030,609.1494,-4471799.0368,2672944.7758,-3668288.9857,1.3923,1.5128,3.2272,2.3026,2.1633,2.1586,0.00000000000,0.00000000000,0.0000,0.00000000000,0.00000000000,0.0000,176366400,0.000,66.175285,18.972784,0.000000,0.000000,5,0,0,0*9f704dad
 */
static void parse_agrica_field(sensor_gps_nmea_t gps_nmea, uint16_t term_number, const char *term)
{
    // subtract 8 to align term numbers with reference manual
    // look for "Unicore Reference Command Manual" to find the specification
    const uint8_t hdr_align = 8;
    if (term_number < hdr_align) {
        // discard header;
        return;
    }
    term_number -= hdr_align;
    // useful for debugging
    //::printf("AGRICA[%u]=%s\n", unsigned(term_number), term);
    switch (term_number) {
    case 10:
        gps_nmea->_agrica.rtk_status = atol(term);
        break;
    case 11:
        gps_nmea->_agrica.heading_status = atol(term);
        break;
    case 25 ... 26:
        ((float *)&gps_nmea->_agrica.vel_NED)[term_number-25] = atof(term);
        break;
    case 27:
        // AGRIC gives velocity up
        gps_nmea->_agrica.vel_NED.z = -atof(term);
        break;
    case 28 ... 30:
        ((float *)&gps_nmea->_agrica.vel_stddev)[term_number-28] = atof(term);
        break;
    case 31:
        gps_nmea->_agrica.lat = atof(term);
        break;
    case 32:
        gps_nmea->_agrica.lng = atof(term);
        break;
    case 33:
        gps_nmea->_agrica.alt = atof(term);
        break;
    case 49:
        gps_nmea->_agrica.itow = atol(term);
        break;
    case 37 ... 39:
        ((float *)&gps_nmea->_agrica.pos_stddev)[term_number-37] = atof(term);
        break;
    case 52:
        gps_nmea->_agrica.undulation = atof(term);
        break;
    }
}

#if GPS_MOVING_BASELINE
/*
  parse a UNIHEADINGA message term

  Example:
    #UNIHEADINGA,79,GPS,FINE,2242,167498200,0,0,18,22;SOL_COMPUTED,L1_INT,2.7889,296.7233,-25.7710,0.0000,0.1127,0.1812,"999",49,37,37,0,3,00,1,51*d50af0ea
 */
static void parse_uniheadinga_field(sensor_gps_nmea_t gps_nmea, uint16_t term_number, const char *term)
{
    const uint8_t hdr_align = 8;
    if (term_number < hdr_align) {
        // discard header;
        return;
    }
    term_number -= hdr_align;
    // useful for debugging
    // ::printf("UNIHEADINGA[%u]=%s\n", unsigned(term_number), term);
    switch (term_number) {
    case 4:
        gps_nmea->_uniheadinga.baseline_length = atof(term);
        break;
    case 5:
        gps_nmea->_uniheadinga.heading = atof(term);
        break;
    case 6:
        gps_nmea->_uniheadinga.pitch = atof(term);
        break;
    case 8:
        gps_nmea->_uniheadinga.heading_sd = atof(term);
        break;
    }
}
#endif // GPS_MOVING_BASELINE

// parse VERSIONA fields
static void parse_versiona_field(sensor_gps_nmea_t gps_nmea, uint16_t term_number, const char *term)
{
    // printf useful for debugging
    // ::printf("VERSIONA[%u]='%s'\n", term_number, term);
#pragma GCC diagnostic push
#if defined(__GNUC__) &&  __GNUC__ >= 10
#pragma GCC diagnostic ignored "-Wstringop-truncation"
#endif
    switch (term_number) {
    case 10:
        strncpy(gps_nmea->_versiona.type, gps_nmea->_term, sizeof(gps_nmea->_versiona.type)-1);
        break;
    case 11:
        strncpy(gps_nmea->_versiona.version, gps_nmea->_term, sizeof(gps_nmea->_versiona.version)-1);
        break;
    case 15:
        strncpy(gps_nmea->_versiona.build_date, gps_nmea->_term, sizeof(gps_nmea->_versiona.build_date)-1);
        break;
    }
#pragma GCC diagnostic pop
}
#endif // MB_GPS_NMEA_UNICORE_ENABLED

/*
  send type specific config strings
 */
static void send_config(sensor_gps_nmea_t gps_nmea)
{
    sensor_gps_backend_t gps_backend = &gps_nmea->backend;

    const enum GPS_Type type = sensor_gps_backend_get_type(gps_backend);
    gps_nmea->_expect_agrica = (type == GPS_TYPE_UNICORE_NMEA ||
                      type == GPS_TYPE_UNICORE_MOVINGBASE_NMEA);
    if (gps._auto_config == GPS_AUTO_CONFIG_DISABLE) {
        // not doing auto-config
        return;
    }
    uint32_t now_ms = time_millis();
    if (now_ms - gps_nmea->last_config_ms < MB_GPS_NMEA_CONFIG_PERIOD_MS) {
        return;
    }
    gps_nmea->last_config_ms = now_ms;
    const uint16_t rate_ms = gps._rate_ms[gps_backend->state->instance];
#if MB_GPS_NMEA_UNICORE_ENABLED
    const float rate_s = rate_ms * 0.001;
#endif
    const uint8_t rate_hz = 1000U / rate_ms;

    switch (sensor_gps_backend_get_type(gps_backend)) {
#if MB_GPS_NMEA_UNICORE_ENABLED
    case GPS_TYPE_UNICORE_MOVINGBASE_NMEA:
        SerialManager_printf(gps_backend->port, "\r\nCONFIG HEADING FIXLENGTH\r\n" \
                     "CONFIG UNDULATION AUTO\r\n" \
                     "CONFIG\r\n" \
                     "UNIHEADINGA %.3f\r\n",
                     rate_s);
        gps_backend->state->gps_yaw_configured = true;

    case GPS_TYPE_UNICORE_NMEA: {
        SerialManager_printf(gps_backend->port, "\r\nAGRICA %.3f\r\n" \
                     "MODE MOVINGBASE\r\n" \
                     "GNGGA %.3f\r\n" \
                     "GNRMC %.3f\r\n",
                     rate_s, rate_s, rate_s);
        if (!gps_nmea->_have_unicore_versiona) {
            // get version information for logging if we don't have it yet
            SerialManager_printf(gps_backend->port, "VERSIONA\r\n");
            if (gps._save_config) {
                // save config changes for fast startup
                SerialManager_printf(gps_backend->port, "SAVECONFIG\r\n");
            }
        }
        break;
    }
#endif // MB_GPS_NMEA_UNICORE_ENABLED

    case GPS_TYPE_HEMI: {
        SerialManager_printf(gps_backend->port, 
        "$JATT,NMEAHE,0\r\n" /* Prefix of GP on the HDT message */      \
        "$JASC,GPGGA,%u\r\n" /* GGA at 5Hz */                            \
        "$JASC,GPRMC,%u\r\n" /* RMC at 5Hz */                            \
        "$JASC,GPVTG,%u\r\n" /* VTG at 5Hz */                            \
        "$JASC,GPHDT,%u\r\n" /* HDT at 5Hz */                            \
        "$JMODE,SBASR,YES\r\n" /* Enable SBAS */,
        rate_hz, rate_hz, rate_hz, rate_hz);
        break;
    }

    case GPS_TYPE_ALLYSTAR:
#if 0
        nmea_printf(port, "$PHD,06,42,UUUUTTTT,BB,0,%u,55,0,%u,0,0,0",
                    unsigned(rate_hz), unsigned(rate_ms));
#endif
        break;

    default:
        break;
    }

#ifdef MB_GPS_NMEA_CUSTOM_CONFIG_STRING
    // allow for custom config strings, useful for peripherals
    SerialManager_printf(gps_backend->port, "%s\r\n", MB_GPS_NMEA_CUSTOM_CONFIG_STRING);
#endif
}


/// 
static bool nmea_read(sensor_gps_backend_t gps_backend)
{
    sensor_gps_nmea_t gps_nmea = (sensor_gps_nmea_t)gps_backend;

    int16_t numc;
    bool parsed = false;

    send_config(gps_nmea);

    numc = SerialManager_rx_available(gps_backend->port);
    while (numc--) {
        char c = (char)SerialManager_read_byte(gps_backend->port);
#if 0
        if (c == '$') {
            console_printf("%d\n", time_millis());
        }
        console_write_byte(c);
#endif
#if MB_GPS_DEBUG_LOGGING_ENABLED
        sensor_gps_backend_log_data((const uint8_t *)&c, 1);
#endif
        if (_decode(gps_nmea, c)) {
            parsed = true;
        }
    }
    return parsed;
}

static const char *nmea_name()
{
    return "NMEA";
}

static bool nmea_is_healthy(sensor_gps_backend_t gps_backend)
{
    sensor_gps_nmea_t gps_nmea = (sensor_gps_nmea_t)gps_backend;

    switch (sensor_gps_backend_get_type(gps_backend)) {
#if MB_GPS_NMEA_UNICORE_ENABLED
    case GPS_TYPE_UNICORE_MOVINGBASE_NMEA:
    case GPS_TYPE_UNICORE_NMEA:
        // we should be getting AGRICA messages
        return gps_nmea->_last_AGRICA_ms != 0;
#endif // MB_GPS_NMEA_UNICORE_ENABLED

    case GPS_TYPE_HEMI:
        // we should be getting HDR for yaw
        return gps_nmea->_last_yaw_ms != 0;

    case GPS_TYPE_ALLYSTAR:
        // we should get vertical velocity and accuracy from PHD
        return gps_nmea->_last_vvelocity_ms != 0 && gps_nmea->_last_vaccuracy_ms != 0;

    default:
        break;
    }
    return true;
}

// get the velocity lag
static bool nmea_get_lag(sensor_gps_backend_t gps_backend, float *lag_sec)
{
    sensor_gps_nmea_t gps_nmea = (sensor_gps_nmea_t)gps_backend;

    switch (sensor_gps_backend_get_type(gps_backend)) {
#if MB_GPS_NMEA_UNICORE_ENABLED
    case GPS_TYPE_UNICORE_MOVINGBASE_NMEA:
    case GPS_TYPE_UNICORE_NMEA:
        *lag_sec = 0.14f;
        break;
#endif // MB_GPS_NMEA_UNICORE_ENABLED

    default:
        *lag_sec = 0.2f;
        break;
    }
    return true;
}

/*------------------------------------test------------------------------------*/
#endif

